#include "cmath"
#include "random"
#include "chrono"
#include "string"
#include "iostream"
#include "ctime"

#if defined _WINDOWS || __WIN32__
#include "windows.h"
#endif

#include "algebra.h"
#include "lgd.h"

#define pi 3.14159265358979323846

/**
 * @brief      return value of the lgd_solver() function
 */
enum lgd_return_enum
{
	LGD_SUCCESS = 0, ///< The iteration terminated successfully.
	LGD_CONVERGENCE = 1, ///< The iteration reached convergence.
	LGD_STOP, ///< The iteration stopped by the progress monitoring function.
	LGD_ALREADY_OPTIMIZIED, ///< The initial solution is already optimized.
	LGD_REACHED_MAX_ITERATIONS, ///< Iteration reached max limit.
	LGD_UNKNOWN_ERROR = -1024, ///< A negative number means a error.
	LGD_INVALID_VARIABLE_SIZE, ///< The variable size is negative.
	LGD_INVALID_OBSERVATION_SIZE, ///< The observation size is negative.
	LGD_INVALID_MAX_ITERATIONS, ///< The maximal iteration times is negative.
	LGD_INVALID_EPSILON, ///< The epsilon is negative.
	LGD_INVALID_BETA, ///< Invalid value for beta.
	LGD_NAN_VALUE, ///< Nan value.
	LGD_INVALID_RANGE, ///< Invalid range of the solution.
	LGD_INVALID_STATISTIC, ///< Can not record standard deviations without calculating mean values.
	LGD_INVALID_POINTER, ///< Invalid pointer.
	LGD_INVALID_VECTOR_SIZE, ///< Invalid vector size.
	LGD_INVALID_ARRAY_SIZE, ///< Invalid array size.
	LGD_OVERDETERMINED, ///< The group of vectors are over determined.
	LGD_INVALID_MU, ///< The parameter mu is negative.
	LGD_OUTOF_MEMORY,
};

lgd_float *lgd_malloc(const int n_size)
{
	lgd_float *x = new lgd_float [n_size];
	return x;
}

void lgd_free(lgd_float* x)
{
	if (x != nullptr) delete[] x;
	x = nullptr;
	return;
}

/**
 * Default parameter for the Lévy-Gradient Descent (L-GD) method.
 */
static const lgd_para defparam = {1000, -1.0, 1.0, 1.5, 0.01, 1e-8, 1.0, -1, 0};

lgd_para lgd_default_parameters()
{
	lgd_para param = defparam;
	return param;
}

void lgd_error_str(int er_index, bool er_throw)
{
#if defined _WINDOWS || __WIN32__
	if (!er_throw)
	{
		if (er_index >= 0)
		{
			SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), FOREGROUND_INTENSITY | FOREGROUND_GREEN);
			std::cerr << "Success! ";
		}
		else
		{
			SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), FOREGROUND_INTENSITY | FOREGROUND_RED);
			std::cerr << "Fail! ";
		}
	}
#else
	if (!er_throw)
	{
		if (er_index >= 0)
			std::cerr << "\033[1m\033[32mSuccess! ";
		else
			std::cerr << "\033[1m\033[31mFail! ";
	}
#endif

	std::string err_str;
	switch (er_index)
	{
		case LGD_SUCCESS:
			err_str = "Success."; break;
		case LGD_CONVERGENCE:
			err_str = "The iteration reached convergence."; break;
		case LGD_STOP:
			err_str = "The iteration stopped by the progress evaluation function."; break;
		case LGD_ALREADY_OPTIMIZIED:
			err_str = "The initial model is already the optimized result."; break;
		case LGD_UNKNOWN_ERROR:
			err_str = "Unknown error."; break;
		case LGD_INVALID_VARIABLE_SIZE:
			err_str = "Invalid solution size."; break;
		case LGD_INVALID_OBSERVATION_SIZE:
			err_str = "Invalid observation size."; break;
		case LGD_INVALID_MAX_ITERATIONS:
			err_str = "Invalid flight times."; break;
		case LGD_INVALID_EPSILON:
			err_str = "The epsilon is negative."; break;
		case LGD_REACHED_MAX_ITERATIONS:
			err_str = "The maximal flight times is reached."; break;
		case LGD_INVALID_BETA:
			err_str = "Invalid value for beta."; break;
		case LGD_NAN_VALUE:
			err_str = "NaN values found."; break;
		case LGD_INVALID_RANGE:
			err_str = "Invalid limits of the solution."; break;
		case LGD_INVALID_STATISTIC:
			err_str = "Can't record standard deviations without recording mean values."; break;
		case LGD_INVALID_POINTER:
			err_str = "Invalid pointer."; break;
		case LGD_INVALID_VECTOR_SIZE:
			err_str = "The vector size must be equal or bigger than two."; break;
		case LGD_INVALID_ARRAY_SIZE:
			err_str = "Incompatible total array size of the orthogonal vectors."; break;
		case LGD_OVERDETERMINED:
			err_str = "The input linearly independent vectors can not be over-determined."; break;
		case LGD_INVALID_MU:
			err_str = "The parameter mu is negative."; break;
		case LGD_OUTOF_MEMORY:
			err_str = "Out of memory."; break;
		default:
			err_str = "Unknown error."; break;
	}

	if (er_throw && er_index < 0) throw err_str;
	else std::cerr << err_str;

#if defined _WINDOWS || __WIN32__
	if (!er_throw)
	{
		if (er_index >= 0)
		{
			SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), 7);
			std::cerr << std::endl;
		}
		else
		{
			SetConsoleTextAttribute(GetStdHandle(STD_ERROR_HANDLE), 7);
			std::cerr << std::endl;
		}	
	}
#else
	if (!er_throw)
	{
		if (er_index >= 0)
			std::cerr << "\033[0m" << std::endl;
		else
			std::cerr << "\033[0m" << std::endl;
	}
#endif

	return;
}

int lgd_levy_length(const lgd_para *param, lgd_float *x)
{
	// set L-GD parameters
	lgd_para para = (param != nullptr) ? (*param) : defparam;

	//check parameters
	if (para.flight_times <= 0) return LGD_INVALID_MAX_ITERATIONS;
	if (para.beta <= 1.0 || para.beta >= 2.0) return LGD_INVALID_BETA;

	lgd_float gamma1 = tgamma(para.beta + 1.0);
	lgd_float gamma2 = tgamma(0.5*(para.beta + 1.0));
	lgd_float stddev_u = pow((gamma1*sin(0.5*pi*para.beta)) / 
		(gamma2*para.beta*pow(2, 0.5*(para.beta-1.0))), 1.0/para.beta);

	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator(seed);
	std::normal_distribution<lgd_float> dist_u(0, stddev_u);
	std::normal_distribution<lgd_float> dist_v(0, para.stddev_v);

	for (int i = 0; i < para.flight_times; i++)
	{
		x[i] = para.alpha * fabs(dist_u(generator)/pow(fabs(dist_v(generator)), 1.0/para.beta));
	}
	return LGD_SUCCESS;
}

/**
 * Declare the utility functions.
 */

/**
 * @brief      generate a standard orthogonal vector group from a set of linearly independent vectors
 *
 * @param[in]  a     pointer a set of linearly independent vectors (stored as a 1D array)
 * @param      e     pointer of the returned orthogonal vector group (stored as a 1D array)
 * @param[in]  t_s   Size of the 1D array. Both a and e's length must be equal to this number (equal or bigger than 4)
 * @param[in]  a_s   Size of the set of vectors (equal or bigger than 2)
 * 
 * @return     status of the function.
 */
int schmidt_orthogonal(const lgd_float *a, lgd_float *e, int t_s, int a_s);

/**
 * @brief      renew mean values by putting in new values
 *
 * @param[in]  old_mean   The old mean
 * @param[in]  old_count  The old count
 * @param[in]  new_input  The new input
 *
 * @return     the new mean value
 */
lgd_float dynamic_average(lgd_float old_mean, int old_count, lgd_float new_input);

/**
 * @brief      renew standard deviation by putting in new values
 *
 * @param[in]  old_stddev  The old standard deviation
 * @param[in]  old_count   The old count
 * @param[in]  old_mean    The old mean
 * @param[in]  new_input   The new input
 * @param      new_mean    The new mean, returned by quote
 *
 * @return     the new standard deviation
 */
lgd_float dynamic_stddev(lgd_float old_stddev, int old_count, lgd_float old_mean, 
	lgd_float new_input, lgd_float &new_mean);

/**
 * @brief      calculate pseudo gradients of a given object function
 *
 * @param[in]  Evafp       Callback function for calculating the objective function.
 * @param      x           Pointer of the solution.
 * @param      orth_dx     Pointer of the orthogonal search group.
 * @param      g           Pointer of the pseudo gradient.
 * @param      x_plus      Temporary solution for the forward differential evaluation.
 * @param      x_minus     Temporary solution for the backward differential evaluation.
 * @param[in]  n_size      Length of the solution.
 * @param[in]  orth_size   Number of the vectors of the search group.
 * @param      instance    The user data sent for the function by the client.
 *
 */
void pseudo_gradient(lgd_evaulate_ptr Evafp, const lgd_float *x, const lgd_float *orth_dx, lgd_float *g, 
	lgd_float *x_plus, lgd_float *x_minus, lgd_float dx_mod, int n_size, int orth_size, void *instance);

/**
 * Begin the solver functions.
 */

int lgd_solver(lgd_float **trace, lgd_float *m_mean, lgd_float *m_stddev, lgd_evaulate_ptr Evafp, 
	lgd_progress_ptr Profp, lgd_float *fx, lgd_float *m, const int n_size, const lgd_para *param, 
	void *instance, const lgd_float *low_limit, const lgd_float *high_limit, const lgd_float *alpha_range)
{
	// set L-GD parameters
	lgd_para para = (param != nullptr) ? (*param) : defparam;

	int ret = LGD_UNKNOWN_ERROR;
	//check parameters
	if (n_size <= 0) return LGD_INVALID_VARIABLE_SIZE;
	if (para.flight_times <= 0) return LGD_INVALID_MAX_ITERATIONS;
	if (para.beta <= 1.0 || para.beta >= 2.0) return LGD_INVALID_BETA;
	if (m_stddev != nullptr && m_mean == nullptr) return LGD_INVALID_STATISTIC;
	if (para.mu <= 0.0) return LGD_INVALID_MU;

	lgd_float gamma1 = tgamma(para.beta + 1.0);
	lgd_float gamma2 = tgamma(0.5*(para.beta + 1.0));
	lgd_float stddev_u = pow((gamma1*sin(0.5*pi*para.beta)) / 
		(gamma2*para.beta*pow(2, 0.5*(para.beta-1.0))), 1.0/para.beta);

	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator(seed);
	std::normal_distribution<lgd_float> dist_u(0, stddev_u);
	std::normal_distribution<lgd_float> dist_v(0, para.stddev_v);
	std::uniform_real_distribution<lgd_float> dist_s(1.0, 2.0);

	lgd_float *new_mean = nullptr;
	lgd_float *m_plus = nullptr, *m_minus = nullptr;
	lgd_float *ran_dm = nullptr, *orth_dm = nullptr;

	lgd_float *g, *g_mem, *g_orth, *best_m, *alphas;
	g = lgd_malloc(n_size);
	g_mem = lgd_malloc(2*n_size);
	g_orth = lgd_malloc(2*n_size);
	best_m = lgd_malloc(n_size);
	alphas = lgd_malloc(n_size);

	if (g == nullptr || g_mem == nullptr || g_orth == nullptr || 
		best_m == nullptr || alphas == nullptr)
	{
		ret = LGD_OUTOF_MEMORY;
		goto func_ends;
	}

	// 初始化参数变化范围为para.alpha
	vecset(alphas, para.alpha, n_size);

	lgd_float tmp_range;
	if (high_limit != nullptr && low_limit != nullptr)
	{
		if (!veccheckbox(high_limit, low_limit, n_size))
		{
			ret = LGD_INVALID_RANGE;
			goto func_ends;
		}

		vecdiff(alphas, high_limit, low_limit, n_size);
		vecscale(alphas, para.alpha, n_size);
	}

	if (alpha_range != nullptr)
	{
		if (!veccheckpos(alpha_range, n_size))
		{
			ret = LGD_INVALID_RANGE;
			goto func_ends;
		}

		vecmul(alphas, alpha_range, para.alpha, n_size);
	}

	if (para.pseudo_grad)
	{
		// set orth_num to n_size if it is smaller than zero
		if (para.orth_num == -1) para.orth_num = n_size;

		m_plus = lgd_malloc(n_size);
		m_minus = lgd_malloc(n_size);
		ran_dm  = lgd_malloc(n_size * para.orth_num);
		orth_dm = lgd_malloc(n_size * para.orth_num);
		if (m_plus == nullptr || m_minus == nullptr || ran_dm == nullptr || orth_dm == nullptr)
		{
			ret = LGD_OUTOF_MEMORY;
			goto func_ends;
		}

		for (int i = 0; i < n_size * para.orth_num; i++)
		{
			ran_dm[i] = dist_s(generator);
		}

		int ret2 = schmidt_orthogonal(ran_dm, orth_dm, n_size * para.orth_num, para.orth_num);
		if (ret2 != 0)
		{
			ret = ret2;
			goto func_ends;
		}

		vecscale(orth_dm, para.mu, n_size*para.orth_num);
	}

	if (m_mean != nullptr && m_stddev != nullptr)
	{
		new_mean = lgd_malloc(n_size);
		if (new_mean == nullptr)
		{
			ret = LGD_OUTOF_MEMORY;
			goto func_ends;
		}
	}

	int best_f;
	lgd_float fx_tmp, direct_mod, levy_length;

	// 计算初始解
	*fx = Evafp(instance, m, g, n_size, 0);
	if (para.pseudo_grad) pseudo_gradient(Evafp, m, orth_dm, g, m_plus, m_minus, para.mu, n_size, para.orth_num, instance);
	// 如果有截至精度则检查初始解是否即符合要求
	if (para.epsilon > 0.0 && *fx <= para.epsilon)
	{
		ret = LGD_ALREADY_OPTIMIZIED;
		goto func_ends;
	}

	// 随机行走的方向 负的梯度方向
	direct_mod = sqrt(vecdot(g, g, n_size));
	// 如果梯度消失 则采用一个随机方向
	if (direct_mod < para.sigma)
	{
		do
		{
			for (int i = 0; i < n_size; i++)
			{
				g[i] = dist_s(generator);
			}

			direct_mod = sqrt(vecdot(g, g, n_size));
		}
		while (direct_mod < para.sigma);
	}

	// 开始飞行
	for (int f = 0; f < para.flight_times; f++)
	{
		// 莱维飞行的步长 注意原公式中无最外层绝对值符号
		// 这是我们需要步长的绝对值 因此取绝对值
		levy_length = fabs(dist_u(generator)/pow(fabs(dist_v(generator)), 1.0/para.beta));

		for (int i = 0; i < n_size; i++)
		{
			m[i] -= levy_length*alphas[i]*g[i]/direct_mod;
			if (m[i] != m[i])
			{
				ret = LGD_NAN_VALUE;
				goto func_ends;
			}
		}

		// 记录梯度方向
		veccpy(g_mem, g, n_size);

		// 这里可以添加取值范围的约束
		if (low_limit != nullptr) vecbtm(m, low_limit, n_size);
		if (high_limit != nullptr) vectop(m, high_limit, n_size);

		// 记录飞行轨迹
		if (trace != nullptr)
		{
			for (int i = 0; i < n_size; i++)
			{
				if (trace[i] != nullptr)
				{
					trace[i][f] = m[i];
				}
			}
		}

		// 记录飞行轨迹
		if (m_mean != nullptr && m_stddev != nullptr)
		{
			for (int i = 0; i < n_size; i++)
			{
				m_stddev[i] = dynamic_stddev(m_stddev[i], f, m_mean[i], m[i], new_mean[i]);
				m_mean[i] = new_mean[i];
			}
		}
		else if (m_mean != nullptr)
		{
			for (int i = 0; i < n_size; i++)
			{
				m_mean[i] = dynamic_average(m_mean[i], f, m[i]);
			}
		}

		// 计算尝试解
		fx_tmp = Evafp(instance, m, g, n_size, 0);
		if (para.pseudo_grad) pseudo_gradient(Evafp, m, orth_dm, g, m_plus, m_minus, para.mu, n_size, para.orth_num, instance);
		if (fx_tmp < *fx)
		{
			*fx = fx_tmp;
			best_f  = f;
			veccpy(best_m, m, n_size);
		}

		if (Profp(instance, *fx, best_m, &para, n_size, f, best_f))
		{
			// 将迭代结果返还给m
			veccpy(m, best_m, n_size);
			ret = LGD_STOP;
			goto func_ends;
		}

		if (para.epsilon > 0.0 && *fx < para.epsilon)
		{
			// 将迭代结果返还给m
			veccpy(m, best_m, n_size);
			ret = LGD_CONVERGENCE;
			goto func_ends;
		}

		// 随机行走的方向 负的梯度方向
		direct_mod = sqrt(vecdot(g, g, n_size));
		// 如果梯度消失 则朝着上一次迭代方向的正交方向走一步
		if (direct_mod < para.sigma)
		{
			for (int i = 0; i < n_size; i++)
			{
				g_mem[i+n_size] = dist_s(generator);
			}

			schmidt_orthogonal(g_mem, g_orth, 2*n_size, 2);

			for (int i = 0; i < n_size; i++)
			{
				g[i] = g_orth[i+n_size];
			}
			direct_mod = 1.0; // 此时的模量为单位模量
		}
	}

	ret = LGD_REACHED_MAX_ITERATIONS;

	// 将迭代结果返还给m
	veccpy(m, best_m, n_size);

	func_ends:
	{
		lgd_free(g);
		lgd_free(g_mem);
		lgd_free(g_orth);
		lgd_free(best_m);
		lgd_free(alphas);
		lgd_free(new_mean);
		lgd_free(m_plus);
		lgd_free(m_minus);
		lgd_free(ran_dm);
		lgd_free(orth_dm);
	}
	return ret;
}

int slgd_solver(lgd_float **trace, lgd_float *m_mean, lgd_float *m_stddev, lgd_evaulate_ptr Evafp, 
	lgd_progress_ptr Profp, lgd_float *fx, lgd_float *m, const int n_size, const int m_size, 
	const lgd_para *param, void *instance, const lgd_float *low_limit, const lgd_float *high_limit, 
	const lgd_float *alpha_range)
{
	// set L-GD parameters
	lgd_para para = (param != nullptr) ? (*param) : defparam;

	int ret = LGD_REACHED_MAX_ITERATIONS;
	//check parameters
	if (n_size <= 0) return LGD_INVALID_VARIABLE_SIZE;
	if (m_size <= 0) return LGD_INVALID_OBSERVATION_SIZE;
	if (para.flight_times <= 0) return LGD_INVALID_MAX_ITERATIONS;
	if (para.beta <= 1.0 || para.beta >= 2.0) return LGD_INVALID_BETA;
	if (m_stddev == nullptr || m_mean == nullptr) return LGD_INVALID_POINTER;
	if (para.mu <= 0.0) return LGD_INVALID_MU;

	lgd_float gamma1 = tgamma(para.beta + 1.0);
	lgd_float gamma2 = tgamma(0.5*(para.beta + 1.0));
	lgd_float stddev_u = pow((gamma1*sin(0.5*pi*para.beta)) / 
		(gamma2*para.beta*pow(2, 0.5*(para.beta-1.0))), 1.0/para.beta);

	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	std::default_random_engine generator(seed);
	std::normal_distribution<lgd_float> dist_u(0, stddev_u);
	std::normal_distribution<lgd_float> dist_v(0, para.stddev_v);
	std::uniform_real_distribution<lgd_float> dist_s(1.0, 2.0);

	lgd_float *g = lgd_malloc(n_size);
	lgd_float *g_mem = lgd_malloc(2*n_size);
	lgd_float *g_orth = lgd_malloc(2*n_size);
	lgd_float *alphas = lgd_malloc(n_size);
	lgd_float *new_mean = lgd_malloc(n_size);
	lgd_float *m_plus = nullptr, *m_minus = nullptr, *ran_dm = nullptr, *orth_dm = nullptr;

	// 初始化一组梯度方向 防止初始解即在驻点位置
	for (int i = 0; i < n_size; i++)
	{
		g_mem[i] = dist_s(generator);
	}

	// 初始化均值与标准差数组
	for (int i = 0; i < n_size; i++)
	{
		m_stddev[i] = m_mean[i] = 0.0;
	}

	// 初始化参数变化范围为para.alpha
	for (int i = 0; i < n_size; i++)
	{
		alphas[i] = para.alpha;
	}

	lgd_float tmp_range;
	if (high_limit != nullptr && low_limit != nullptr)
	{
		for (int i = 0; i < n_size; i++)
		{
			tmp_range = high_limit[i] - low_limit[i];
			if (tmp_range <= 0.0)
			{
				ret = LGD_INVALID_RANGE; goto func_ends;
			}
			else alphas[i] = tmp_range * para.alpha;
		}
	}

	if (alpha_range != nullptr)
	{
		for (int i = 0; i < n_size; i++)
		{
			if (alpha_range[i] <= 0.0)
			{
				ret = LGD_INVALID_RANGE; goto func_ends;
			}
			else alphas[i] = alpha_range[i] * para.alpha;
		}
	}

	// 初始化伪梯度计算数组
	if (para.pseudo_grad)
	{
		// set orth_num to n_size if it is smaller than zero
		if (para.orth_num == -1) para.orth_num = n_size;

		m_plus = lgd_malloc(n_size);
		m_minus = lgd_malloc(n_size);
		ran_dm  = lgd_malloc(n_size * para.orth_num);
		orth_dm = lgd_malloc(n_size * para.orth_num);
		for (int i = 0; i < n_size * para.orth_num; i++)
		{
			ran_dm[i] = dist_s(generator);
		}

		int ret2 = schmidt_orthogonal(ran_dm, orth_dm, n_size * para.orth_num, para.orth_num);
		if (ret2 != 0)
		{
			ret = ret2; goto func_ends;
		}

		for (int i = 0; i < n_size * para.orth_num; i++)
		{
			orth_dm[i] *= para.mu;
		}
	}

	// 开始飞行
	lgd_float tmp_fx, direct_mod, levy_length;
	for (int f = 0; f < para.flight_times; f++)
	{
		// 莱维飞行的步长 注意原公式中无最外层绝对值符号
		// 这是我们需要步长的绝对值 因此取绝对值
		levy_length = fabs(dist_u(generator)/pow(fabs(dist_v(generator)), 1.0/para.beta));

		*fx = 0.0;
		for (int s = 0; s < m_size; s++)
		{
			// 计算尝试解
			tmp_fx = Evafp(instance, m, g, n_size, s);
			if (para.pseudo_grad) pseudo_gradient(Evafp, m, orth_dm, g, m_plus, m_minus, para.mu, n_size, para.orth_num, instance);

			*fx += tmp_fx; // 记录每一轮观测值循环中的最大目标函数值

			// 随机行走的方向 负的梯度方向
			direct_mod = 0.0;
			for (int i = 0; i < n_size; i++)
			{
				direct_mod += g[i]*g[i];
			}
			direct_mod = sqrt(direct_mod);

			// 如果梯度消失 则朝着上一次迭代方向的正交方向走一步
			if (direct_mod < para.sigma)
			{
				for (int i = 0; i < n_size; i++)
				{
					g_mem[i+n_size] = dist_s(generator);
				}

				schmidt_orthogonal(g_mem, g_orth, 2*n_size, 2);

				for (int i = 0; i < n_size; i++)
				{
					g[i] = g_orth[i+n_size];
				}
				direct_mod = 1.0; // 此时的模量为单位模量
			}

			for (int i = 0; i < n_size; i++)
			{
				m[i] -= levy_length*alphas[i]*g[i]/direct_mod;
				if (m[i] != m[i])
				{
					ret = LGD_NAN_VALUE; goto func_ends;
				}
			}

			// 记录梯度方向
			for (int i = 0; i < n_size; i++)
			{
				g_mem[i] = g[i];
			}

			// 这里可以添加取值范围的约束
			if (low_limit != nullptr)
			{
				for (int i = 0; i < n_size; i++)
				{
					if (m[i] < low_limit[i])
					{
						m[i] = low_limit[i];
					}
				}
			}

			if (high_limit != nullptr)
			{
				for (int i = 0; i < n_size; i++)
				{
					if (m[i] > high_limit[i])
					{
						m[i] = high_limit[i];
					}
				}
			}

			// 统计单次遍历中的模型与标准差
			for (int i = 0; i < n_size; i++)
			{
				m_stddev[i] = dynamic_stddev(m_stddev[i], f*m_size + s, m_mean[i], m[i], new_mean[i]);
				m_mean[i] = new_mean[i];
			}

			// 记录飞行轨迹
			if (trace != nullptr)
			{
				for (int i = 0; i < n_size; i++)
				{
					if (trace[i] != nullptr)
					{
						trace[i][f*m_size + s] = m[i];
					}
				}
			}
		}
		*fx /= m_size;

		if (Profp(instance, *fx, nullptr, &para, n_size, f, 0))
		{
			ret = LGD_STOP; goto func_ends;
		}

		if (para.epsilon > 0 && *fx < para.epsilon)
		{
			ret = LGD_CONVERGENCE; goto func_ends;
		}
	}

	func_ends:
	{
		lgd_free(g);
		lgd_free(g_mem);
		lgd_free(g_orth);
		lgd_free(alphas);
		lgd_free(new_mean);
		lgd_free(m_plus);
		lgd_free(m_minus);
		lgd_free(ran_dm);
		lgd_free(orth_dm);
	}
	return ret;
}

/**
 * Definition the utility functions.
 */

lgd_float dynamic_average(lgd_float old_mean, int old_count, lgd_float new_input)
{
	return (old_mean*old_count + new_input)/(old_count + 1.0);
}

lgd_float dynamic_stddev(lgd_float old_stddev, int old_count, lgd_float old_mean, lgd_float new_input, lgd_float &new_mean)
{
	new_mean = dynamic_average(old_mean, old_count, new_input);
	return sqrt(old_stddev*old_stddev*old_count/(old_count + 1.0) 
		+ old_count*pow(old_mean - new_input, 2.0)/pow(old_count + 1.0, 3.0)
		+ pow(new_input - new_mean, 2.0)/(old_count + 1.0));
}

int schmidt_orthogonal(const lgd_float *a, lgd_float *e, int t_s, int a_s)
{
	if (a == nullptr || e == nullptr) return LGD_INVALID_POINTER;
	if (a_s <= 1) return LGD_INVALID_VECTOR_SIZE;
	if (t_s%a_s != 0 || t_s <= 3) return LGD_INVALID_ARRAY_SIZE;

	int len = t_s/a_s;
	if (len < a_s) return LGD_OVERDETERMINED;

	lgd_float ae, ee;
	for (int i = 0; i < a_s; i++)
	{
		for (int l = 0; l < len; l++)
		{
			e[l + i*len] = a[l + i*len];
		}

		for (int m = 0; m < i; m++)
		{
			ae = ee = 0.0;
			for (int n = 0; n < len; n++)
			{
				ae += a[n + i*len] * e[n + m*len];
				ee += e[n + m*len] * e[n + m*len];
			}

			for (int n = 0; n < len; n++)
			{
				e[n + i*len] -= e[n + m*len] * ae/ee;
			}
		}
	}

	for (int i = 0; i < a_s; i++)
	{
		ee = 0.0;
		for (int l = 0; l < len; l++)
		{
			ee += e[l + i*len] * e[l + i*len];
		}
		ee = sqrt(ee);

		for (int l = 0; l < len; l++)
		{
			e[l + i*len] /= ee;
		}
	}
	return 0;
}

void pseudo_gradient(lgd_evaulate_ptr Evafp, const lgd_float *x, const lgd_float *orth_dx, lgd_float *g, 
	lgd_float *x_plus, lgd_float *x_minus, lgd_float dx_mod, int n_size, int orth_size, void *instance)
{
	for (int i = 0; i < n_size; i++)
	{
		g[i] = 0.0;
	}

	lgd_float fx_plus, fx_minus;
	for (int o = 0; o < orth_size; o++)
	{
		for (int i = 0; i < n_size; i++)
		{
			x_plus[i] = x[i] + orth_dx[i + o*n_size];
			x_minus[i]= x[i] - orth_dx[i + o*n_size];
		}

		fx_plus = Evafp(instance, x_plus, nullptr, n_size, 0);
		fx_minus= Evafp(instance, x_minus, nullptr, n_size, 0);

		for (int i = 0; i < n_size; i++)
		{
			g[i] += orth_dx[i + o*n_size]*(fx_plus - fx_minus)/(2.0*dx_mod*dx_mod);
		}
	}
	return;
}

LGD_Solver::LGD_Solver()
{
	param_ = lgd_default_parameters();
}

int LGD_Solver::Progress(lgd_float fx, const lgd_float *best_x, const lgd_para *param, 
	const int n_size, const int curr_k, const int best_k)
{
	std::clog << "\rflight times: " << curr_k << ", object-value: " << fx << ", found at flight time: " << best_k;
	return 0;
}

void LGD_Solver::set_lgd_parameter(const lgd_para &in_param)
{
	param_ = in_param;
	return;
}

void LGD_Solver::Minimize(lgd_float **trace, lgd_float *m_mean, lgd_float *m_stddev, 
	lgd_float *fx, lgd_float *m, const int n_size, const lgd_float *low_limit, 
	const lgd_float *high_limit, const lgd_float *alpha_range, 
	bool verbose, bool er_throw)
{
	clock_t start = clock();
	int ret = lgd_solver(trace, m_mean, m_stddev, _Evaluate, _Progress, fx, 
		m, n_size, &param_, this, low_limit, high_limit, alpha_range);
	clock_t end = clock();
	lgd_float costime = 1000*(end-start)/(double)CLOCKS_PER_SEC;

	if (!er_throw)
	{
		std::clog << std::endl << "Time cost: " << costime << " ms" << std::endl;
	}

	if (verbose) lgd_error_str(ret, er_throw);
	else if (ret < 0) lgd_error_str(ret, er_throw);
	return;
}

void LGD_Solver::MinimizeStochastic(lgd_float **trace, lgd_float *m_mean, lgd_float *m_stddev, 
	lgd_float *fx, lgd_float *m, const int n_size, const int m_size, const lgd_float *low_limit, 
	const lgd_float *high_limit, const lgd_float *alpha_range, bool verbose, bool er_throw)
{
	clock_t start = clock();
	int ret = slgd_solver(trace, m_mean, m_stddev, _Evaluate, _Progress, fx, 
		m, n_size, m_size, &param_, this, low_limit, high_limit, alpha_range);
	clock_t end = clock();
	lgd_float costime = 1000*(end-start)/(double)CLOCKS_PER_SEC;

	if (!er_throw)
	{
		std::clog << std::endl << "Time cost: " << costime << " ms" << std::endl;
	}

	if (verbose) lgd_error_str(ret, er_throw);
	else if (ret < 0) lgd_error_str(ret, er_throw);
	return;
}
